# 附有参数的条件平差 Adjustment of condition equation with parameter

import numpy as np
from numpy import sin, sqrt, deg2rad
from MathUtils import *


def inv(m):
	return np.linalg.inv(m)


P_A = np.array([1000.0, 0.0])
P_B = np.array([1000.0, 1732.0])

L = np.array([
	deg2rad(dms2dec((60, 0, 3))),
	deg2rad(dms2dec((60, 0, 2))),
	deg2rad(dms2dec((60, 0, 4))),
	deg2rad(dms2dec((59, 59, 57))),
	deg2rad(dms2dec((59, 59, 56))),
	deg2rad(dms2dec((59, 59, 59)))
])


X0 = deg2rad(dms2dec((30, 0, 0)))

S_AB = sqrt((P_B - P_A).dot(P_B - P_A))
S_BD = 1000

a = (sin(L[1-1] - X0) * sin(L[3-1] + L[5-1]) * sin(L[4-1])) / (sin(L[2-1] + L[4-1]) * sin(L[5-1]) * sin(X0))
b = S_AB*sin(X0) / (S_BD*sin(L[3-1] + L[5-1]))
p = 206265

A = np.mat([
	[1, 1, 1, 0, 0, 0],
	[0, 0, 0, 1, 1, 1],
	[a*cot(L[1-1] - X0), -a*cot(L[2-1] + L[4-1]), a*cot(L[3-1] + L[5-1]), a*cot(L[4-1]) - a*cot(L[2-1] + L[4-1]), -a*cot(L[5-1]) + a*cot(L[3-1] + L[5-1]), 0],
	[0, 0, -b*cot(L[3-1] + L[5-1]), 0, -b*cot(L[3-1] + L[5-1]), 0]
])

B = np.mat([
	0,
	0,
	-a*cot(L[1-1] - X0) - a*cot(X0),
	b*cot(X0)
]).transpose()

W = np.mat([
	-9,
	8,
	(1 - a)*p,
	(1 - b)*p
]).transpose()

print("A = \n", A)
print("B = \n", B)
print("W = \n", W)

P = np.eye(6, 6)
Naa = A*P*A.transpose()
print("Naa = \n", Naa)

Nbb = B.transpose()*inv(Naa)*B
print("Nbb = \n", Nbb)

xh = inv(Nbb)*B.transpose()*inv(Naa)*W
print("xh = \n", xh)

K = inv(Naa)*(W - B*xh)
print("K = \n", K)

Q = P
V = Q*A.transpose()*K
print("V = \n", V)

Lh = L + V
print("Lh = \n", Lh)

Xh = X0 + xh
print("Xh = \n", Xh)

Qxx = inv(Nbb)
print("Qxx = \n", Qxx)

Qxl = -inv(Nbb)*B.transpose()*inv(Naa)*A*Q
print("Qxl = \n", Qxl)

Qll = Q - Q*A.transpose()*(inv(Naa) - inv(Naa)*B*inv(Nbb)*B.transpose()*inv(Naa))*A*Q
print("Qll = \n", Qll)

Fl = np.mat([1, 0, 0, 0, 0, 0]).transpose()
Fx = np.mat([-1])

Qff = Fl.transpose()*Qll*Fl + Fl.transpose()*Qxl.transpose()*Fx + Fx.transpose()*Qxl*Fl + Fx.transpose()*Qxx*Fx
print("Qff = \n", Qff)


sigma_0h = np.sqrt((V.transpose().dot(V)) / (6 - 3))
print("sigma_0h =", sigma_0h)
sigma_f = sigma_0h*sqrt(Qff)
print("sigma_f =", sigma_f)
